package Behaviors;

import lejos.robotics.navigation.Pose;
import lejos.robotics.subsumption.Behavior;
import sharedData.DebugItem;
import sharedData.MapData;
import sharedData.SharedConstants;
import NXTExplorer.ExplorerBot;
import NXTExplorer.SensorParameter;

public class FollowWall implements Behavior {

	private boolean suppressed = false;
	private final int ROTATE_ANGLE = 30;
	private final int TRAVEL_DISTANCE = 10;
	private SensorParameter paramObj;
	

	public FollowWall(SensorParameter params) {
		paramObj = params;
	}

	@Override
	public boolean takeControl() {
		sendDebug("Follow Wall taking control");
		return ExplorerBot.USSRMode < 0;
	}

	@Override
	public void action() {
//		suppressed = false;
//		paramObj.getPilot().forward();
//
//		while (!suppressed && ExplorerBot.USSRMode < 0) {
//
//			if (ExplorerBot.USSRMode == -1) {
//				// We turned right, use left sensor.
//
//				// Save the last know distance to the wall
//				ExplorerBot.lastDistanceKnown = paramObj.getLeftUssr().getDistance();
//				
//				// DEBUG
//				sendDebug("USSRMode -1, following wall on left");
//
//				if (paramObj.getLeftUssr().getDistance() <= SharedConstants.USSR_MAGNET_RANGE_LOWER) {
//
//					// DEBUG
//					sendDebug("Distance is too close to the wall, turning outwards (right)");
//
//					paramObj.getPilot().rotate(ROTATE_ANGLE);
//					paramObj.getPilot().travel(TRAVEL_DISTANCE);
//					paramObj.getPilot().rotate(-ROTATE_ANGLE);
//					
//				} else if (paramObj.getLeftUssr().getDistance() > SharedConstants.USSR_MAGNET_RANGE_UPPER) {
//
//					// DEBUG
//					sendDebug("Nothing to see on the left side");
//
//					ExplorerBot.USSRMode = 1;
//					break;
//				}
//			} else if (ExplorerBot.USSRMode == -2) {
//				// We turned left, use right sensor.
//
//				// Save the last know distance to the wall
//				ExplorerBot.lastDistanceKnown = paramObj.getRightUssr().getDistance();
//				
//				// DEBUG
//				sendDebug("USSRMode -2, following wall on right");
//
//				if (paramObj.getRightUssr().getDistance() <= SharedConstants.USSR_MAGNET_RANGE_LOWER) {
//
//					// DEBUG
//					sendDebug("Distance is too close to the wall, turning outwards (left)");
//
//					paramObj.getPilot().rotate(-ROTATE_ANGLE);
//					paramObj.getPilot().travel(TRAVEL_DISTANCE);
//					paramObj.getPilot().rotate(ROTATE_ANGLE);
//					
//				} else if (paramObj.getRightUssr().getDistance() > SharedConstants.USSR_MAGNET_RANGE_UPPER) {
//
//					// DEBUG
//					sendDebug("Nothing to see on the right side");
//
//					ExplorerBot.USSRMode = 2;
//					break;
//				}
//			}
//
//			handlePosition(paramObj.getPoseProvider().getPose());
//
//			Thread.yield();
//		}
//		
//		handlePosition(paramObj.getPoseProvider().getPose());
	}

	private void handlePosition(Pose pose) {
		if (pose == null)
			return;
		MapData data = new MapData(SharedConstants.DATA_TYPE_MAP_DATA,
				pose.getX(), pose.getY(), pose.getHeading(), paramObj
						.getLeftUssr().getRange(), paramObj.getRightUssr()
						.getRange(), paramObj.getCenterUssr().getRange());

		paramObj.getQueue().add(data);
	}

	@Override
	public void suppress() {
		suppressed = true;
		paramObj.getPilot().stop();
	}

	public void sendDebug(String msg) {
		paramObj.getQueue().add(
				new DebugItem(SharedConstants.DATA_TYPE_DEBUG, paramObj
						.getPoseProvider().getPose().getX(), paramObj
						.getPoseProvider().getPose().getY(), paramObj
						.getPoseProvider().getPose().getHeading(), msg));
	}

}
